Práctica 3: Localización EKF y Mapeo - Rodríguez, Maximiliano

Filtro de Kalman Extendido (EKF)

El EKF funciona igual que el filtro de Kalman, pero en lugar de asumir que el sistema se comporta linealmente, linealiza las funciones no lineales alrededor del estado actual usando derivadas (Jacobianos).
Supongamos que el sistema se describe con:
Donde y son ruidos gaussianos.
El EKF se puede dividir en dos pasos, los cuales son predicción y corrección.

Predicción

El EFK realiza la predicción de la siguiente manera:
En orden de implementar este filtro, se calculó la matriz jacobiana de la funcion de movimiento g según el Modelo de Odometría. Este modelo tiene las siguientes ecuaciones:
Donde
Según lo definido anteriormente, podemos calcular la matriz :
De la misma manera, se calculó :
Se implementó el algoritmo de la siguiente manera:
function [mu_, sigma_] = prediction_step(mu, sigma, u)
% Updates the belief, i. e., mu and sigma, according to the motion model
%
% u: odometry reading (r1, t, r2)
% mu: 3 x 1 vector representing the mean (x, y, theta) of the normal distribution
% sigma: 3 x 3 covariance matrix of the normal distribution
 
% Compute the noise-free motion. This corresponds to the function g, evaluated
% at the state mu.
theta = mu(3);
mu_ = mu + [u.t * cos(theta + u.r1);
u.t * sin(theta + u.r1);
u.r1 + u.r2];
 
% Compute the Jacobian of g with respect to the state
G = [1, 0, -u.t * sin(theta + u.r1);
0, 1, u.t * cos(theta + u.r1);
0, 0, 1];
 
V = [-u.t*sin(theta+u.r1), cos(theta+u.r1), 0;
u.t*cos(theta+u.r1), sin(theta+u.r1), 0;
1, 0, 1];
 
% Motion noise
Q = [0.2, 0, 0;
0, 0.2, 0;
0, 0, 0.02];
 
sigma_ = G * sigma * G.' + V*Q*V.';
end

Corrección

El EKF realiza la corrección de la siguiente manera:
Luego, para implementar el algoritmo, se calculó la matriz jacobiana de la función de medición h sin ruido de un sensor que solo mide distancia.
Sabiendo que la función de medición mide la distancia al landmark:
Si se define , entonces:
Luego, ya que el sensor solo mide rango.
Se implementó el algoritmo de la siguiente manera:
function [mu_, sigma_] = correction_step(mu, sigma, z, l)
mu_ = mu;
sigma_ = sigma;
 
R = 0.5^2; % Varianza escalar, ya que sólo hay rango
 
for i = 1:length(z)
j = z(i).id;
mx = l(j).x;
my = l(j).y;
 
dx = mx - mu_(1);
dy = my - mu_(2);
q = dx^2 + dy^2;
 
% Medición esperada
z_hat = sqrt(q);
 
% Jacobiano de h respecto al estado
H = [-dx / sqrt(q), -dy / sqrt(q), 0]; % 1x3
 
% Kalman
S = H * sigma_ * H.' + R;
K = sigma_ * H.' / S;
 
z_real = z(i).range;
innovation = z_real - z_hat;
 
% Corrección
mu_ = mu_ + K * innovation;
mu_(3) = atan2(sin(mu_(3)), cos(mu_(3))); % Normalizar theta
sigma_ = (eye(3) - K * H) * sigma_;
end
end
Se obtuvo el siguiente recorrido:
KALMAN.gif

Mapeo con poses conocidas

Se requirió lo siguiente:
"Un robot debe construir un mapa de grilla de ocupación (celdas c0, . . . , cn) de un entorno unidimensional usando una secuencia de mediciones de un sensor de distancia. Asumir el siguiente modelo de sensor: cada celda de la grilla con una distancia menor que la distancia medida se asume ocupada con una probabilidad de p = 0,3. Cada celda más allá de la distancia medida se asume ocupada con una probabilidad de p = 0,6. Las celdas ubicadas a más de 20cm por detrás de la distancia medida no cambian su probabilidad de ocupación."
Para calcular el mapa de grilla de ocupación resultante, se utilizó el modelo de sensor inverso. Primero se inicializaron los valores necesarios para el algoritmo:
c = 0:10:200; % Vectores de posiciones en el espacio (de 0 a 200, con paso de 10)
mediciones = [101, 82, 91, 112, 99, 151, 96, 85, 99, 105]; % Mediciones del sensor 1
mediciones2 = [101, 99, 97, 102, 99, 100, 96, 104, 99, 105]; % Mediciones del sensor 2
l = zeros(1,21); % Inicializa un vector para almacenar los valores logarítmicos de la creencia (l) para cada posición.
La variable l guarda los log-odds, que permite actualizar creencias de forma aditiva. El log-odds se define como:
, y la inversa es
Para cada celda deberemos actualizar donde se comporta de la siguiente manera:
Luego, se calcula la creencia para el primer sensor:
for i = 1:size(mediciones,2)
for j = 1:size(c,2)
if c(j) >= mediciones(i) - 20 && c(j) <= mediciones(i) + 20
l(j) = l(j) + inv_sensor_model(mediciones(i), c(j));
end
end
end
 
m = 1 - 1./(1 + exp(l));
En este bloque, el código itera sobre todas las mediciones del primer sensor (mediciones).
Para cada medición, se compara con las posiciones en c. Si la posición c(j) está dentro de un rango de 20 unidades alrededor de la medición (mediciones(i)), se calcula el valor de creencia logarítmica utilizando el modelo de sensor inverso inv_sensor_model.
Una vez terminado el ciclo for, se crea la función inv_sensor_model para que devuelva el log-odd de la forma que habiamos descripto anteriormente.
function l = inv_sensor_model(medicion, posicion)
if (posicion < medicion)
l = log(0.3/(1-0.3));
else
l = log(0.6/(1-0.6));
end
end
Se obtuvieron los siguientes beliefs: